package edu.centenary.centenaryController;
import java.io.*;
import java.util.*;
import java.net.*;
import java.lang.*;
/**
* Manages the menu system which allows the user to connect to the server, start a robot, and command it via AI or text commands.  Starts all other threads and closes them on exit.
*/
public class CentController extends SleeperThread {
	 /**
	 * A socket that connects to the simulator server
	 */
    public static Socket centSocket = null;
	 /**
	 * A Print Writer that sends messages to the server
	 */
    public static PrintWriter out = null;
	 /**
	 * A Buffered Reader that gets messages from the server
	 */
    public static BufferedReader in = null;
	 /**
	 * A Buffered Reader that gets the user's input so it can be saved and sent to the server with "out".
	 */
    public static BufferedReader stdIn = new BufferedReader(new InputStreamReader(System.in));
	 /**
	 * Sensor Data method object
	 */
    public static SensorData sd = new SensorData();
	 /**
	 * Used to set the robot type and adjust features relating to specific robots.  Functionality currently incomplete.
	 */
    public static String robotType = "P2AT";
	 /**
	 * A robots drive type for robot specific commands. Functionality currently incomplete.
	 */
    public static String robotDrive = "DRIVE";
	 
	 
	 public static ArrayList<Connection> cons = new ArrayList<Connection>();

	 public static Connection currentCon = null;

    public static ArrayList<String> robotname= new ArrayList<String>();
	 /**
	 * Tracks the number of names given out
	 */
	 public static int nameTicket = 0;
	 
	 
	 /**
	  * Lists user options and calls correct method for each case.
	  * @return the option selected by the user
	  */
    public static int mainMenu() {
        System.out.println("Type the number for the desired option \n" +
                           "1 Setup Connection \n" + "2 Start Robot \n" + "3 Manual Robot Control \n"
                           + "4 AI Robot Control \n" + "5 Connect to Another Robot \n" + "6 Register with Base Station \n"
									 + "7 Quit \n");
			
        int n = Utilities.getInteger("");

        switch (n) {
        case 1:
            setupConnection();
            break;
        case 2:
            if (currentCon != null) {
                startRobot();
            } else {
                System.out.println("Connect before starting robot. \n");
            }

            break;
        case 3:
		  		if( currentCon != null){
				currentCon.iwSetUp();
            manualControl();
				}else{
				System.out.println("No connection found");
				}
            break;
        case 4:
            if (currentCon != null) {
					currentCon.iwSetUp();
                aiControl();



            } else {
                System.out.println("Start a robot before activating the AI. \n");
            }
            break;
				
		  case 5:
		  		if(currentCon == null){
					System.out.println("Create a connection before switching connections \n");
					}else{
		  			System.out.println("Select Robot Connection. \n");
					for (int i = 0; i < cons.size(); i++){
						System.out.println((i) + " " + cons.get(i).getMyName());
					}
					boolean correctInput = false;
					while(!correctInput){
						int s = Utilities.getInteger("Input desired connection #. \n");;
						if(s > cons.size()){
							System.out.println("Invalid Input. Try another number.");
						}else{
							currentCon = cons.get(s);
							System.out.println("Switched to: " + s + " " + currentCon.getMyName());
							out = currentCon.getOut();
							correctInput = true;
						}
					}
				System.out.println("\n");
				}
				break;
		  case 6:
		  		//Register all robots
		  		for(int i = 0; i < cons.size(); i++){
		  			cons.get(i).getOut().println("REGISTER {Robot" + cons.get(i).getMyName() + "} {IP" + cons.get(i).getIp() + "}\r\n");
					cons.get(i).getOut().println("LISTEN {Robot" + cons.get(i).getMyName() + "} {Port" + cons.get(i).getPort()+ "}\r\n");
					System.out.println(cons.get(i).getIp() + " " + cons.get(i).getPort());

					for(int j = 0; j < cons.size(); j++){
						cons.get(i).getOut().println("OPEN {Robot" + cons.get(i).getMyName() + "} {Port" + cons.get(i).getPort() + "} {Robot" +
						cons.get(j).getMyName() + "} {Port" + cons.get(j).getPort()+ "}\r\n");
					
					}	
				}
		  		break;
        case 7:
            System.out.println("Exiting Program");
            break;
        default:
            System.out.println("Invalid Selection. Try again");
        }
        //Set up if statements to call methods 1-6
        return n;
    }
    /**
	 * The first step which sets the socket IP address for the server running the simulator
	 */
    public static void setupConnection() {
       // if (isConnected == false) {

            System.out.println("You have selected to establish a connection.");

            String ip = Utilities.getString("Input IP address \n");
            int port = 3000;
            //int port = Utilities.getInteger("Input port number: Ex. 3000 for USARSim \n");

            try {
                
					 String roname = robotname.get((nameTicket % robotname.size()))+ (nameTicket / robotname.size());
					 nameTicket++;
					 Connection c = new Connection(ip, port, roname);
					 
					 centSocket = c.getSocket();
					 out = c.getOut();
					 in = c.getIn();
					 sd = c.getSd();
					 cons.add(c);
					 currentCon = c;

            } catch (UnknownHostException e) {
                System.err.println("Don't know about host: "+ ip);
                return;
            } catch (IOException e) {
                System.err.println("Couldn't get I/O for "
                                   + ip);
                return;
            }
        //}else {
         //   System.out.println("You have already established a connection. \n");
        //}
    }



    /**
	 * Creates a robot of the user's choice at a location in the simulator.
	 */
    public static void startRobot() {

        if (!currentCon.getRobotInit()) {
            System.out.println("Please type the number of the robot to start. \n"+
										 "Note that the AI will only work for options 1-3. \n" +
                               "1 P2AT \n"+
                               "2 StereoP2AT \n"+
                               "3 P2DX \n"+
                               "4 ATRVjr \n"+
                               "5 Hummer \n"+
										 "6 Kurt2D \n"+
										 "7 Kenaf \n" +
										 "8 AIBO \n" +
										 "9 ComStation \n");//+
           
            int r = Utilities.getInteger("");
            switch (r) {
            case 1:
                robotType = "P2AT";
                robotDrive = "DRIVE";
                break;
            case 2:
                robotType = "StereoP2AT";
                robotDrive = "DRIVE";
                break;
            case 3:
                robotType = "P2DX";
                robotDrive = "DRIVE";
                break;
            case 4:
                robotType = "ATRVJr";
                robotDrive = "DRIVE";
                break;
            case 5:
                robotType = "Hummer";
                robotDrive = "DRIVE";
                break;
				case 6:
					robotType = "Kurt2D";
					robotDrive = "DRIVE";
					break;
				case 7:
					robotType = "kenaf";
					robotDrive = "DRIVE";
					break;
				case 8:
					robotType = "ERS";
					robotDrive = "DRIVE";
					break;
				case 9:
					robotType = "ComStation";
					robotDrive = "DRIVE";
					break;
            }
            //Asks server for robot starting location and poses (Player Start positions in UnrealED)
            out.println("GETSTARTPOSES \r\n");
            sleep(100);
            System.out.println("Type the number of your choice: \n" +
                               "1: Select an existing starting pose \n" +
                               "2: Specify your own pose");
				//Sets robot's name  **Deal with running out of names**
				
				
				
				
            int n = Utilities.getInteger("");
            double x = 0;
            double y = 0;
            double z = 0;
            double roll = 0;
            double pitch = 0;
            double yaw = 0;

            if (n == 1) {
                System.out.println(sd.numberOfPoses() + " poses found!");
                for (int i = 0; i < sd.numberOfPoses(); i ++) {
                    Pose cpose = sd.getPose(i);
                    System.out.println("Pose " + i + ": (X,Y,Z) " + cpose.getX() + "," + cpose.getY() + "," + cpose.getZ() + "\n" +
                                       "(Roll, Pitch, Yaw) " + cpose.getRoll() + "," + cpose.getPitch() + "," + cpose.getYaw() + "\n" + "\n");
                    n = 200;
                }
                while ( n > (sd.numberOfPoses() -1)) {
                    n = Utilities.getInteger("Input # for desired pose. \n");
                    if (n > (sd.numberOfPoses() -1)) {
                        System.out.println("Incorrect Input.  Please try again");
                    }
                }
                Pose cpose = sd.getPose(n);
                x = cpose.getX();
                y = cpose.getY();
                z = cpose.getZ();
                roll = cpose.getRoll();
                pitch = cpose.getPitch();
                yaw = cpose.getYaw();
                currentCon.setRobotInit(true);


            } else if (n == 2) {
                System.out.println("Input starting coordinates (x,y,z)");

                x = Utilities.getDouble("X \n");
                y = Utilities.getDouble("Y \n");
                z = Utilities.getDouble("Z \n");

                System.out.println("Input rotation (roll(x),pitch(y),yaw(z)");

                roll = Utilities.getDouble("Roll \n");
                pitch = Utilities.getDouble("Pitch \n");
                yaw = Utilities.getDouble("Yaw \n");
                currentCon.setRobotInit(true);
					 
                // Set up specific sensors based on robot type --if (robotType.equals("P2AT") etc
            } else {
                System.out.println("Invalid selection \n");
            }
            out.println("INIT " + "{ClassName USARBot." + robotType + "} " + "{Name Skeeter} " + "{Location " + x + "," + y + "," + z + "} "+
                        "{Rotation " + roll + "," + pitch + "," + yaw + "}\r\n");
				
            //System.out.println("INIT " + "{ClassName USARBot." + robotType + "} " +"{Name Skeeter} "+ "{Location " + x + "," + y + "," + z + "}");
			//	sd.setInitialX(x);
				//sd.setInitialY(y);
				//sd.setInitialZ(z);
				//sd.setInitialRoll(roll);
				//sd.setInitialPitch(pitch);
				//sd.setInitialYaw(yaw);

        }// else {
         //   System.out.println("Sorry, but only one robot may be created by each controller \n");
        //}


    }
    /**
	 * Allows the user to control the robot by manually typing in the commands found in the USARSim manual
	 */
    public static void manualControl() {
        System.out.println("You have selected to control your robot.");
		  System.out.println("Type QUIT to return to the menu");
		  String userInput = "";
		  
        try {
            while ((userInput = stdIn.readLine()) != null && !userInput.equals("QUIT")) {
                out.println(userInput + "\r\n");
            }
        } catch (IOException e) {
            System.out.println("Bad readLine from socket");
            return;
        }
    }

    /**
	 * Controls robot via AI routine.
	 */
    public static void aiControl() {
        System.out.println("You have selected to control via AI.");
        currentCon.aiStart();
    }

	 /**
	 * Gets all robot names from file and puts into an array
	 */
	 public static void roboName(){
		String file = "edu/centenary/centenaryController/robotnames.txt";
		BufferedReader nameReader;
		String str;
	   try{
			nameReader = new BufferedReader(new FileReader(file));
	 		while((str = nameReader.readLine()) != null){
				//System.out.println(str);
				robotname.add(str);
			}
			nameReader.close();
		}catch(FileNotFoundException e){
				System.out.println("File not found");
		}catch (IOException e){
				System.out.println("IO Error");
		}
	 }
	 /**
	 * The main method which displays the menu until exit.  Closes all running threads on exit.
	 */
    public static void main(String[] args)throws IOException {
		  roboName();
        int n =0;

        while (n != 7) {
            n = mainMenu();
        }
        //Be sure to close ai before exiting
        //System.out.println("Beginning exit");
		  for(int i = 0; i < cons.size(); i++){
		  //Try putting this into Connection object
		  
		  cons.get(i).closeConnections();
        //System.out.println("Closing centSocket");
        //System.out.println("Exit Complete!");
    }
	 //System.out.println("Closing in");
            stdIn.close();
	 }
}
